/*
 * FreeRTOS Kernel <DEVELOPMENT BRANCH>
 * Copyright (C) 2021 Amazon.com, Inc. or its affiliates.  All Rights Reserved.
 *
 * SPDX-License-Identifier: MIT
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
 * the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 * https://www.FreeRTOS.org
 * https://github.com/FreeRTOS
 *
 */

/*-----------------------------------------------------------
 * Implementation of functions defined in portable.h for the Cygnal port.
 *----------------------------------------------------------*/

/* Standard includes. */
#include <string.h>

/* Scheduler includes. */
#include "FreeRTOS.h"
#include "task.h"

#define initTaskStack_ROMLarg()                                                    \
    do                                                                             \
    {                                                                              \
        /* Simulate how the stack would look after a call to the scheduler tick */ \
        /* ISR. The return address that would have been pushed by the MCU. */      \
        ulAddress = (uint32_t)pxCode;                                              \
        *pxTopOfStack = (StackType_t)ulAddress;                                    \
        ulAddress >>= 8;                                                           \
        pxTopOfStack++;                                                            \
        *pxTopOfStack = (StackType_t)(ulAddress);                                  \
                                                                                   \
        /* Press the stack in order, and the low address is entered first */       \
        /* DPL DPH DR4 DR8 DR12 DR16 DR20 DR24 DR28 */                             \
        for (index = 0; index < 30; index++)                                       \
        {                                                                          \
            pxTopOfStack++;                                                        \
            *pxTopOfStack = 0;                                                     \
        }                                                                          \
                                                                                   \
        /* The function parameters will be passed in the DR0 register as */        \
        /* a four byte generic pointer is used. */                                 \
                                                                                   \
        pxTopOfStack++;                                                            \
        ulAddress = ( uint32_t )pvParameters;                                      \
        *pxTopOfStack = ( StackType_t )( ulAddress >> 24 ); /* R0 */               \
        *pxTopOfStack++;                                                           \
        *pxTopOfStack = ( StackType_t )( ulAddress >> 16 ); /* R1 */               \
        pxTopOfStack++;                                                            \
        *pxTopOfStack = ( StackType_t )( ulAddress >>  8 ); /* R2 */               \
        pxTopOfStack++;                                                            \
        *pxTopOfStack = ( StackType_t )( ulAddress >>  0 ); /* R3 */               \
                                                                                   \
        /* Press the stack in order, and the low address is entered first */       \
        /* PSW PSW1 DPXL */                                                        \
                                                                                   \
        for (index = 0; index < 3; index++)                                        \
        {                                                                          \
            pxTopOfStack++;                                                        \
            *pxTopOfStack = 0;                                                     \
        }                                                                          \
    } while (0)

#define initTaskStack_ROMHurge()                                                   \
    do                                                                             \
    {                                                                              \
        /* Simulate how the stack would look after a call to the scheduler tick */ \
        /* ISR. The return address that would have been pushed by the MCU. */      \
        ulAddress = (uint32_t)pxCode;                                              \
        *pxTopOfStack = (StackType_t)0xFF;                                         \
        pxTopOfStack++;                                                            \
        *pxTopOfStack = (StackType_t)((uint16_t)ulAddress >> 8);                   \
        pxTopOfStack++;                                                            \
        *pxTopOfStack = (StackType_t)(ulAddress);                                  \
                                                                                   \
        /* Press the stack in order, and the low address is entered first */       \
        /* DPXL DR4 DR8 DR12 DR16 DR20 DR24 DR28 */                                \
        for (index = 0; index < 29; index++)                                       \
        {                                                                          \
            pxTopOfStack++;                                                        \
            *pxTopOfStack = 0;                                                     \
        }                                                                          \
                                                                                   \
        /* The function parameters will be passed in the DR0 register as */        \
        /* a four byte generic pointer is used. */                                 \
                                                                                   \
        pxTopOfStack++;                                                            \
        ulAddress = (uint32_t)pvParameters;                                        \
        *pxTopOfStack = ( StackType_t )( ulAddress >> 24 ); /* R0 */               \
        *pxTopOfStack++;                                                           \
        *pxTopOfStack = ( StackType_t )( ulAddress >> 16 ); /* R1 */               \
        pxTopOfStack++;                                                            \
        *pxTopOfStack = ( StackType_t )( ulAddress >>  8 ); /* R2 */               \
        pxTopOfStack++;                                                            \
        *pxTopOfStack = ( StackType_t )( ulAddress >>  0 ); /* R3 */               \
                                                                                   \
        /* Press the stack in order, and the low address is entered first */       \
        /* DPL DPH PSW PSW1  */                                                    \
                                                                                   \
        for (index = 0; index < 4; index++)                                        \
        {                                                                          \
            pxTopOfStack++;                                                        \
            *pxTopOfStack = 0;                                                     \
        }                                                                          \
    } while (0)

#define portSwitch_Context_ROMLarg()                                                       \
    do                                                                                     \
    {                                                                                      \
        pxStack = (StackType_t *)((uint16_t)SPH << 8 | SP);                                \
        *((StackType_t **)pxCurrentTCB) = pxStack - 3;                                     \
                                                                                           \
        /* Call the standard scheduler context switch function. */                         \
        vTaskSwitchContext();                                                              \
        pxStack++;                                                                         \
        SP = (uint8_t)pxStack;                                                             \
        SPH = (uint16_t)pxStack >> 8;                                                      \
        *pxStack-- = (uint16_t)vPortYield_inISR >> 8;                                      \
        *pxStack-- = (uint8_t)vPortYield_inISR;                                            \
        *pxStack-- = (uint32_t)vPortYield_inISR >> 16;                                     \
        *pxStack = 0; \
        pxStack -= 2; \
        *pxStack = DPXL; \
        /* PSW1 does not need to write to the return stack again */                        \
        /* and the task switching context will be replaced later */                        \
        /* *pxStack = PSW1; */                                                             \
        /*The pxstack is subtracted by 1, pointing to the location where PSW1 is stored */ \
        /* and then subtracted by 38 to the first address of the interrupt stack */        \
        pxStack -= 37;                                                                     \
                                                                                           \
        /* Address of pressing stack before moving into interrupt  */                      \
        *(pxStack + 0) = *(pxStack + 2);                                                   \
        *(pxStack + 1) = *(pxStack + 3);                                                   \
                                                                                           \
        /* Copy DPH DPL */                                                                 \
        *(pxStack + 2) = DPH;                                                              \
        *(pxStack + 3) = DPL;                                                              \
    } while (0)

//      PUSH     DPXL
//      PUSH     DPH
//      PUSH     DPL
//      PUSH     --
#define portSwitch_Context_ROMHurge()                                                      \
    do                                                                                     \
    {                                                                                      \
        pxStack = (StackType_t *)((uint16_t)SPH << 8 | SP);                                \
        *((StackType_t **)pxCurrentTCB) = pxStack - 2;                                     \
                                                                                           \
        /* Call the standard scheduler context switch function. */                         \
        vTaskSwitchContext();                                                              \
                                                                                           \
        pxStack++;                                                                         \
        SP = (uint8_t)pxStack;                                                             \
        SPH = (uint16_t)pxStack >> 8;                                                      \
        *pxStack-- = (uint16_t)vPortYield_inISR >> 8;                                      \
        *pxStack-- = (uint8_t)vPortYield_inISR;                                            \
        *pxStack-- = (uint32_t)vPortYield_inISR >> 16;                                     \
        *pxStack-- = 0;                                                                    \
        *pxStack-- = DPL;                                                                  \
        *pxStack   = DPH;                                                                  \
        /* PSW1 does not need to write to the return stack again */                        \
        /* and the task switching context will be replaced later */                        \
        /* *pxStack = PSW1; */                                                             \
        /*The pxstack is subtracted by 1, pointing to the location where PSW1 is stored */ \
        /* and then subtracted by 38 to the first address of the interrupt stack */        \
        pxStack -= 37;                                                                     \
        /* Address of pressing stack before moving into interrupt  */                      \
        *(pxStack + 0) = *(pxStack + 1);                                                   \
        *(pxStack + 1) = *(pxStack + 3);                                                   \
        /* Copy DPH DPL */                                                                 \
        *(pxStack + 3) = DPXL;                                                             \
    } while (0)

/* Take over the interrupt service processing of timer 0 
by the compiler and point to the interrupt service program implemented under port */
#ifndef configUSE_ROMHUGE
    #pragma asm
        vTimer0  SEGMENT  CODE OFFS 0BH
        RSEG vTimer0
        LJMP vTimer0ISR?_
    #pragma endasm
#else
    #if (configUSE_ROMHUGE == 1)
        #pragma asm
            vTimer0  SEGMENT  CODE OFFS 0BH
            RSEG vTimer0
            EJMP vTimer0ISR??
        #pragma endasm
    #else
        #pragma asm
            vTimer0  SEGMENT  CODE OFFS 0BH 
            RSEG vTimer0
            LJMP vTimer0ISR?_
        #pragma endasm
    #endif
#endif
        
/* We require the address of the pxCurrentTCB variable, but don't want to know
any details of its type. */
typedef void TCB_t;
extern volatile TCB_t * pxCurrentTCB;

/*
 * Setup the hardware to generate an interrupt off timer 2 at the required
 * frequency.
 */
static void prvSetupTimerInterrupt( void );

static StackType_t *pxStack;

/*
 * See header file for description.
 */

StackType_t *pxPortInitialiseStack( StackType_t *pxTopOfStack, TaskFunction_t pxCode, void *pvParameters )
{
    StackType_t *pxStartOfStack;
    uint32_t ulAddress;
    uint8_t index;
    /* Leave space to write the size of the stack as the first byte. */
    pxStartOfStack = pxTopOfStack;
    
    #ifndef configUSE_ROMHUGE
    {
        initTaskStack_ROMLarg();
    }
    #else
    {
        #if (configUSE_ROMHUGE == 1)
        {
            initTaskStack_ROMHurge();
        }
        #else
        {
            initTaskStack_ROMLarg();
        }
        #endif
    }
    #endif

    /* we return the top of the stack */
    
    return pxTopOfStack;
}

/*-----------------------------------------------------------*/

/*
 * See header file for description.
 */

BaseType_t xPortStartScheduler( void )
{
    /* Setup timer 2 to generate the RTOS tick. */
    
    prvSetupTimerInterrupt();

    /* Make sure we start with the expected SFR page.  This line should not
    really be required. */
    
    pxStack =  ( StackType_t * ) *( ( StackType_t ** ) pxCurrentTCB );
    SP  = (  uint8_t ) pxStack;
    SPH = ( uint16_t ) pxStack >> 8;
    
    /* Copy the stack for the first task to execute from XRAM into the stack,
    restore the task context from the new stack, then start running the task. */
    
    #ifndef configUSE_ROMHUGE
    {
        #pragma asm
        POP      DPXL
        POP      PSW
        POP      PSW1
        POP      DR0
        POP      DR4
        POP      DR8
        POP      DR12
        POP      DR16
        POP      DR20
        POP      DR24
        POP      DR28
        POP      DPL
        POP      DPH
        RET
        #pragma endasm
    }
    #else
    {
        #if (configUSE_ROMHUGE == 1)
        {
            __asm POP      DPL
            __asm POP      DPH
            __asm POP      PSW
            __asm POP      PSW1
            __asm POP      DR0
            __asm POP      DR4
            __asm POP      DR8
            __asm POP      DR12
            __asm POP      DR16
            __asm POP      DR20
            __asm POP      DR24
            __asm POP      DR28
            __asm POP      DPXL
            __asm ERET
        }
        #else
        {
            #pragma asm
            POP      DPXL
            POP      PSW
            POP      PSW1
            POP      DR0
            POP      DR4
            POP      DR8
            POP      DR12
            POP      DR16
            POP      DR20
            POP      DR24
            POP      DR28
            POP      DPL
            POP      DPH
            RET
            #pragma endasm
        }
        #endif
    }
    #endif

    /* Should never get here! */

    return pdTRUE;
}

/*-----------------------------------------------------------*/

#ifndef configUSE_ROMHUGE
   void vPortEndScheduler(void) { }
#else
    #if (configUSE_ROMHUGE == 0)
     void vPortEndScheduler(void) { }
    #endif
#endif

/*
 * Manual context switch.
 */

void vPortYield( void )
{
    #ifndef configUSE_ROMHUGE
    {
        #pragma asm
        CLR      EA
        PUSH     DPH
        PUSH     DPL
        PUSH     DR28
        PUSH     DR24
        PUSH     DR20
        PUSH     DR16
        PUSH     DR12
        PUSH     DR8
        PUSH     DR4
        PUSH     DR0
        PUSH     PSW1
        PUSH     PSW
        MOV      PSW, #00H
        PUSH     DPXL
        #pragma endasm
    }
    #else
    {
        #if (configUSE_ROMHUGE == 1)
        {
            #pragma asm
            CLR      EA
            PUSH     DPXL
            PUSH     DR28
            PUSH     DR24
            PUSH     DR20
            PUSH     DR16
            PUSH     DR12
            PUSH     DR8
            PUSH     DR4
            PUSH     DR0
            PUSH     PSW1
            PUSH     PSW
            MOV      PSW, #00H
            PUSH     DPH
            PUSH     DPL
            #pragma endasm
        }
        #else
        {
            #pragma asm
            CLR      EA
            PUSH     DPH
            PUSH     DPL
            PUSH     DR28
            PUSH     DR24
            PUSH     DR20
            PUSH     DR16
            PUSH     DR12
            PUSH     DR8
            PUSH     DR4
            PUSH     DR0
            PUSH     PSW1
            PUSH     PSW
            MOV      PSW, #00H
            PUSH     DPXL
            #pragma endasm
        }
        #endif
    }
    #endif

    *((StackType_t **)pxCurrentTCB) = ( StackType_t * ) (( uint16_t ) SPH << 8 | SP);

    /* Call the standard scheduler context switch function. */
    
    vTaskSwitchContext();

    /* Copy the stack of the task about to execute from XRAM into RAM and
    restore it's context ready to run on exiting. */
    
    pxStack =  ( StackType_t * ) *( ( StackType_t ** ) pxCurrentTCB );
    SP  = ( uint8_t ) pxStack;
    SPH = (( uint16_t )pxStack) >> 8;

    #ifndef configUSE_ROMHUGE
    {
        #pragma asm
        POP      DPXL
        POP      PSW
        POP      PSW1
        POP      DR0
        POP      DR4
        POP      DR8
        POP      DR12
        POP      DR16
        POP      DR20
        POP      DR24
        POP      DR28
        POP      DPL
        POP      DPH
        SETB     EA
        #pragma endasm
    }
    #else
    {
        #if (configUSE_ROMHUGE == 1)
        {
            #pragma asm
            POP      DPL
            POP      DPH
            POP      PSW
            POP      PSW1
            POP      DR0
            POP      DR4
            POP      DR8
            POP      DR12
            POP      DR16
            POP      DR20
            POP      DR24
            POP      DR28
            POP      DPXL
            SETB     EA
            #pragma endasm
        }
        #else
        {
            #pragma asm
            POP      DPXL
            POP      PSW
            POP      PSW1
            POP      DR0
            POP      DR4
            POP      DR8
            POP      DR12
            POP      DR16
            POP      DR20
            POP      DR24
            POP      DR28
            POP      DPL
            POP      DPH
            SETB     EA
            #pragma endasm
        }
        #endif
    }
    #endif

}

#if (configUSE_PREEMPTION == 1)
void vPortYield_inISR( void )
{
    pxStack =  ( StackType_t * ) *( ( StackType_t ** ) pxCurrentTCB );
    SP  = ( uint8_t ) pxStack;
    SPH = ( ( uint16_t ) pxStack) >> 8;

    #ifndef configUSE_ROMHUGE
    {
        #pragma asm
        POP      DPXL
        POP      PSW
        POP      PSW1
        POP      DR0
        POP      DR4
        POP      DR8
        POP      DR12
        POP      DR16
        POP      DR20
        POP      DR24
        POP      DR28
        POP      DPL
        POP      DPH
        SETB     EA
        #pragma endasm
    }
    #else
    {
        #if (configUSE_ROMHUGE == 1)
        {
            #pragma asm
            POP      DPL
            POP      DPH
            POP      PSW
            POP      PSW1
            POP      DR0
            POP      DR4
            POP      DR8
            POP      DR12
            POP      DR16
            POP      DR20
            POP      DR24
            POP      DR28
            POP      DPXL
            SETB     EA
            #pragma endasm
        }
        #else
        {
            #pragma asm
            POP      DPXL
            POP      PSW
            POP      PSW1
            POP      DR0
            POP      DR4
            POP      DR8
            POP      DR12
            POP      DR16
            POP      DR20
            POP      DR24
            POP      DR28
            POP      DPL
            POP      DPH
            SETB     EA
            #pragma endasm
        }
        #endif
    }
    #endif
}
#endif
/*-----------------------------------------------------------*/

#if configUSE_PREEMPTION == 1
    // PUSH     PSW1              4
    // PUSH     PC[23:16]
    // PUSH     PC[07:00]
    // PUSH     PC[15:08]
    void vTimer0ISR( void )
    {
        #pragma asm
        CLR      EA
        PUSH     DR28
        PUSH     DR24
        PUSH     DR20
        PUSH     DR16
        PUSH     DR12
        PUSH     DR8
        PUSH     DR4
        PUSH     DR0
        PUSH     PSW1
        PUSH     PSW
        MOV      PSW, #00H
//      PUSH     DPXL
//      PUSH     DPH
//      PUSH     DPL
//      PUSH     --
        PUSH     DR56
        #pragma endasm

        P2 = ~P2;
        /* Preemptive context switch function triggered by the timer 0 ISR.
        This does the same as vPortYield() (see above) with the addition
        of incrementing the RTOS tick count. */

        if( xTaskIncrementTick() != pdFALSE )
        {
//            __asm  MOV      DPXL, #11H
//            __asm  MOV      DPH, #22H
//            __asm  MOV      DPL, #33H
            #ifndef configUSE_ROMHUGE
            {
                portSwitch_Context_ROMLarg();
            }
            #else
            {
                #if (configUSE_ROMHUGE == 1)
                {
                    portSwitch_Context_ROMHurge();
                }
                #else
                {
                    portSwitch_Context_ROMLarg();
                }
                #endif
            }
            #endif
            
            #pragma asm
            RETI
            #pragma endasm
        }

        #pragma asm
//      POP      --
//      POP      DPL
//      POP      DPH
//      POP      DPXL
        POP      DR56
        POP      PSW
        POP      PSW1
        POP      DR0
        POP      DR4
        POP      DR8
        POP      DR12
        POP      DR16
        POP      DR20
        POP      DR24
        POP      DR28
        SETB     EA
        RETI
        #pragma endasm
        
        // POP     PC[15:08]
        // POP     PC[07:00]
        // POP     PC[23:16]
        // POP     PSW1
    }

#else
    void vTimer0ISR( void )
    {
        #pragma asm
        CLR      EA
        PUSH     DR28
        PUSH     DR24
        PUSH     DR20
        PUSH     DR16
        PUSH     DR12
        PUSH     DR8
        PUSH     DR4
        PUSH     DR0
        PUSH     PSW1
        PUSH     PSW
        MOV      PSW, #00H
//      PUSH     DPXL
//      PUSH     DPH
//      PUSH     DPL
//      PUSH     --
        PUSH     DR56
        #pragma endasm
        
        xTaskIncrementTick();
        
        #pragma asm
//      POP      --
//      POP      DPL
//      POP      DPH
//      POP      DPXL
        POP      DR56
        POP      PSW
        POP      PSW1
        POP      DR0
        POP      DR4
        POP      DR8
        POP      DR12
        POP      DR16
        POP      DR20
        POP      DR24
        POP      DR28
        SETB     EA
        RETI
        #pragma endasm
    }
#endif

/*-----------------------------------------------------------*/

#define MAIN_Fosc       24000000UL
#define Timer0_Reload   (MAIN_Fosc / 1000)      //Timer 0 

static void prvSetupTimerInterrupt( void )
{
    /* Constants calculated to give the required timer capture values. */
    TR0 = 0;

    #if (Timer0_Reload < 64)
        #error "Timer0"

    #elif ((Timer0_Reload/12) < 65536UL)
        ET0 = 1;
        PT0 = 1;
        TMOD &= ~0x03;
        TMOD |= 0;
    //  TMOD |=  0x04;
        TMOD &= ~0x04;
    //  INTCLKO |=  0x01;
        INTCLKO &= ~0x01;

    #if (Timer0_Reload < 65536UL)
        AUXR |=  0x80;  //1T mode
        TH0 = (uint8_t)((65536UL - Timer0_Reload) / 256);
        TL0 = (uint8_t)((65536UL - Timer0_Reload) % 256);
    #else
        AUXR &= ~0x80;  //12T mode
        TH0 = (uint8_t)((65536UL - Timer0_Reload/12) / 256);
        TL0 = (uint8_t)((65536UL - Timer0_Reload/12) % 256);
    #endif

    TR0 = 1;

    #else
        #error "Timer0 "
    #endif
}




